#include <jni.h>
#include <android/log.h>
#include <string>
#include <cmath>
#include <ctime>
#include <unistd.h>
#include "MahonyAHRS.h"
#include "SixAxisImuPose.h"
#include "LevelOneComplementaryFiltering.h"
#include "KFFiltering.h"

extern "C" JNIEXPORT jstring JNICALL
Java_com_example_agsensor_MainActivity_stringFromJNI(
        JNIEnv* env,
        jobject /* this */) {
    std::string hello = "Hello from C++";
    return env->NewStringUTF(hello.c_str());
}

extern "C" JNIEXPORT jfloatArray JNICALL
Java_com_example_agsensor_MainActivity_doMahonyAHRSupdateIMU(
        JNIEnv *env,
        jobject thiz,
        jfloatArray accelArray,jfloatArray gyroArray,jint methodType/*0:MahonyAHRS or 1:SixAxisImuPose*/){
    IMU_Accel mAcc;
    IMU_Gyro mGyro;
    IMU_Angle mAngle;
    jfloatArray angleArray;
    float angles[3];

    jfloat* accelArray1 = env->GetFloatArrayElements(accelArray,NULL);
    jfloat* gyroArray1 = env->GetFloatArrayElements(gyroArray,NULL);
    mAcc.x =  accelArray1[0];
    mAcc.y =  accelArray1[1];
    mAcc.z =  accelArray1[2];
    mGyro.x = gyroArray1[0];
    mGyro.y = gyroArray1[1];
    mGyro.z = gyroArray1[2];

    if( methodType == 0 || methodType == 1 ) {
        if (methodType == 0)
            SixAxisImuUpdate(mGyro.x, mGyro.y, mGyro.z, mAcc.x, mAcc.y, mAcc.z, &mAngle);
        else if (methodType == 1)
            MahonyAHRSupdateIMU(mAcc, mGyro, &mAngle);

        angles[0] = mAngle.roll * 57.3/*一弧度对应角度*/;//x
        angles[1] = mAngle.pitch * 57.3/*一弧度对应角度*/;//y
        angles[2] = mAngle.yaw * 57.3/*一弧度对应角度*/;//z
    }else if( methodType == 2 ){  //一阶互补滤波
        double angle_ax = atan2(mAcc.y, mAcc.z)*57.3;     //加速度得到的角度
        double angle_ay = atan2(mAcc.x, mAcc.z)*57.3;     //加速度得到的角度

        angles[0] = (jfloat)LevelOneComplementaryRoll(angle_ax, mGyro.x); //一阶互补滤波
        angles[1] =  (jfloat)LevelOneComplementaryPitch(angle_ay, mGyro.y);
        angles[2] = 0;
    }else if( methodType == 3 ){ //简易卡尔曼滤波
        double angle_ay = atan2(mAcc.x, mAcc.z)*57.3;     //加速度得到的角度
        Kalman_Filter(angle_ay, mGyro.y);//卡尔曼滤波

        angles[0] = 0;
        angles[1] =  getPitch();
        angles[2] = 0;
    }

    angleArray = env->NewFloatArray(3);
    env->SetFloatArrayRegion(angleArray, 0, 3, angles);

    env->ReleaseFloatArrayElements(accelArray, accelArray1, 0);
    env->ReleaseFloatArrayElements(gyroArray, gyroArray1, 0);

    return angleArray;
}